from time import sleep
from gpiozero import  AngularServo
from gpiozero.pins.pigpio import PiGPIOFactory

class Servo:
    def __init__(self,Pin,init_angle):
        self.pin=Pin
        self.s = AngularServo(self.pin, initial_angle=init_angle,min_angle=0, max_angle=180,pin_factory= PiGPIOFactory())
          
    def turn_anti_clockwise(self,step_angle):
        # anti-clockwise
        for _ in range(step_angle):
            angle=self.s.angle-1
            if angle<self.s.min_angle:
               angle=self.s.min_angle
               self.s.angle=angle
               break
            else:
               self.s.angle=angle
            sleep(0.05)   
           
    def turn_clockwise(self,step_angle):
        # clockwise
        for _ in range(step_angle):
            angle=self.s.angle+1
            if angle>self.s.max_angle:
               angle=self.s.max_angle
               self.s.angle=angle
               break
            else:
               self.s.angle=angle
            sleep(0.05)   
       
    def turn_to_angle(self,angle):
        diff_angle=self.s.angle-angle
        for _ in range(abs(int(diff_angle))):
            if  diff_angle>0:
               angle=self.s.angle-1
            else:
               angle=self.s.angle+1
            self.s.angle=angle
            sleep(0.1)   
         

if  __name__=="__main__":
    #vertical_servo=Servo(18,90) # BCM 18
    #sleep(1)
    #vertical_servo.turn_clockwise(20)
    ##sleep(1)
    #vertical_servo.turn_anti_clockwise(20)
    #sleep(1)
    #vertical_servo.set_angle(90)
    #sleep(1)
    #
    #horizontal_servo=Servo(19,90) # BCM GPIO24
    #horizontal_servo.turn_clockwise(20)
    #sleep(1)
    #horizontal_servo.turn_anti_clockwise(20)
    #sleep(1)
    #horizontal_servo.set_angle(90)
    #sleep(1)
    laser_vertical_Pin=19
    laser_vertical_init_angle=60
    laser_vertical_servo=Servo(laser_vertical_Pin,laser_vertical_init_angle)
    sleep(1)
    each_step_angle=20
    laser_vertical_servo.turn_clockwise(each_step_angle)
    sleep(2)
    #laser_vertical_servo.turn_anti_clockwise(each_step_angle)
    #sleep(2)
    #laser_vertical_servo.turn_to_angle(60)
    #sleep(2)
    #laser_vertical_servo.turn_to_angle(90)
    #sleep(2)

 

    
    
    